#ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_JPS_PLANNER_H
#define ROBORTS_PLANNING_GLOBAL_PLANNER_JPS_PLANNER_H
/**************************************
 * tzgj-jps-planner
 * Author:LJN
 **************************************/

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

#include "alg_factory/algorithm_factory.h"
#include "state/error_code.h"
#include "costmap/costmap_interface.h"

#include "../global_planner_base.h"
#include "jps_basis/data_utils.h"
#include <jps_planner/distance_map_planner/distance_map_planner.h>
#include <jps_planner/jps_planner/jps_planner.h>

#include <algorithm>
#include <iterator>
#include <string>
#include <vector>

namespace roborts_global_planner{

    class JpsPlanner : public GlobalPlannerBase{

 public:
    /**
     * @brief  Default constructor for the JumpPointSearchROS object
     */
    // JumpPointSearchROS();

    /**
     * @brief  Constructor for the JumpPointSearchROS object
     * @param  name The name of this planner
     * @param  costmap A pointer to the ROS wrapper of the costmap to use
     */
    JpsPlanner(CostmapPtr costmap_ptr);

    virtual ~JpsPlanner();

    /**
     * @brief  Constructor for the JumpPointSearchROS object
     * @param  name The name of this planner
     * @param  costmap A pointer to the costmap to use
     * @param  global_frame The global frame of the costmap
     */
    // JumpPointSearchROS(std::string name, costmap_2d::Costmap2D* costmap, std::string global_frame);

    /**
     * @brief  Initialization function for the JumpPointSearchROS object
     * @param  name The name of this planner
     * @param  costmap A pointer to the ROS wrapper of the costmap to use for planning
     */
    void initialize(CostmapPtr costmap_ros);


    /**
     * @brief Given a goal pose in the world, compute a plan
     * @param start The start pose
     * @param goal The goal pose
     * @param plan The plan... filled by the planner
     * @return True if a valid plan was found, false otherwise
     */
    roborts_common::ErrorInfo Plan(const geometry_msgs::PoseStamped &start,
                           const geometry_msgs::PoseStamped &goal,
                           std::vector<geometry_msgs::PoseStamped> &path);

  protected:
    /**
     * @brief Store a copy of the current costmap in \a costmap.  Called by makePlan.
     */
    // <<< TODO: Use smart pointer!!! >>>
    CostmapPtr costmap_ptr_;
    roborts_costmap::Costmap2D* costmap_;
    // costmap_2d::Costmap2DROS* costmap_ros_;
    

    std::shared_ptr<JPS::OccMapUtil> map_util_;
    std::shared_ptr<JPSPlanner2D> jps_planner_;

    ros::Publisher plan_pub_;
    ros::Publisher jps_path_pub_;
    ros::Publisher force_point_pub_;
    ros::Subscriber pose_sub_;
    bool initialized_;

  private:
    void clearRobotCell(const geometry_msgs::PoseStamped& global_pose, unsigned int mx, unsigned int my);
    bool debug_;
    boost::mutex mutex_;
    ros::ServiceServer make_plan_srv_;
    std::string global_frame_;
    };

  roborts_common::REGISTER_ALGORITHM(GlobalPlannerBase,
                                 "jps_planner",
                                 JpsPlanner,
                                 std::shared_ptr<roborts_costmap::CostmapInterface>);
  }

    void clearRobotCell(const geometry_msgs::PoseStamped& global_pose,
                                          unsigned int mx, unsigned int my);

#endif